﻿using System;
using GeoAPI.Geometries;
using YJKGridTopologyCal.Mathematics;

namespace YJKGridTopologyCal.Algorithm
{
	public class CGAlgorithms3D
	{
		public static double Distance(Coordinate p0, Coordinate p1)
		{
			if (double.IsNaN(p0.Z) || double.IsNaN(p1.Z))
			{
				return p0.Distance(p1);
			}
			double num = p0.X - p1.X;
			double num2 = p0.Y - p1.Y;
			double num3 = p0.Z - p1.Z;
			return Math.Sqrt(num * num + num2 * num2 + num3 * num3);
		}

		public static double DistancePointSegment(Coordinate p, Coordinate A, Coordinate B)
		{
			if (A.Equals3D(B))
			{
				return CGAlgorithms3D.Distance(p, A);
			}
			double num = (B.X - A.X) * (B.X - A.X) + (B.Y - A.Y) * (B.Y - A.Y) + (B.Z - A.Z) * (B.Z - A.Z);
			if (double.IsNaN(num))
			{
				throw new ArgumentException("Ordinates must not be NaN");
			}
			double num2 = ((p.X - A.X) * (B.X - A.X) + (p.Y - A.Y) * (B.Y - A.Y) + (p.Z - A.Z) * (B.Z - A.Z)) / num;
			if (num2 <= 0.0)
			{
				return CGAlgorithms3D.Distance(p, A);
			}
			if (num2 >= 1.0)
			{
				return CGAlgorithms3D.Distance(p, B);
			}
			double num3 = A.X + num2 * (B.X - A.X);
			double num4 = A.Y + num2 * (B.Y - A.Y);
			double num5 = A.Z + num2 * (B.Z - A.Z);
			double num6 = p.X - num3;
			double num7 = p.Y - num4;
			double num8 = p.Z - num5;
			return Math.Sqrt(num6 * num6 + num7 * num7 + num8 * num8);
		}

		public static double DistanceSegmentSegment(Coordinate A, Coordinate B, Coordinate C, Coordinate D)
		{
			if (A.Equals3D(B))
			{
				return CGAlgorithms3D.DistancePointSegment(A, C, D);
			}
			if (C.Equals3D(B))
			{
				return CGAlgorithms3D.DistancePointSegment(C, A, B);
			}
			double num = Vector3D.Dot(A, B, A, B);
			double num2 = Vector3D.Dot(A, B, C, D);
			double num3 = Vector3D.Dot(C, D, C, D);
			double num4 = Vector3D.Dot(A, B, C, A);
			double num5 = Vector3D.Dot(C, D, C, A);
			double num6 = num * num3 - num2 * num2;
			if (double.IsNaN(num6))
			{
				throw new ArgumentException("Ordinates must not be NaN");
			}
			double num7;
			double num8;
			if (num6 <= 0.0)
			{
				num7 = 0.0;
				if (num2 > num3)
				{
					num8 = num4 / num2;
				}
				else
				{
					num8 = num5 / num3;
				}
			}
			else
			{
				num7 = (num2 * num5 - num3 * num4) / num6;
				num8 = (num * num5 - num2 * num4) / num6;
			}
			if (num7 < 0.0)
			{
				return CGAlgorithms3D.DistancePointSegment(A, C, D);
			}
			if (num7 > 1.0)
			{
				return CGAlgorithms3D.DistancePointSegment(B, C, D);
			}
			if (num8 < 0.0)
			{
				return CGAlgorithms3D.DistancePointSegment(C, A, B);
			}
			if (num8 > 1.0)
			{
				return CGAlgorithms3D.DistancePointSegment(D, A, B);
			}
			double x = A.X + num7 * (B.X - A.X);
			double y = A.Y + num7 * (B.Y - A.Y);
			double z = A.Z + num7 * (B.Z - A.Z);
			double x2 = C.X + num8 * (D.X - C.X);
			double y2 = C.Y + num8 * (D.Y - C.Y);
			double z2 = C.Z + num8 * (D.Z - C.Z);
			return CGAlgorithms3D.Distance(new Coordinate(x, y, z), new Coordinate(x2, y2, z2));
		}
	}
}
